#include "PLL.h"
#include "math.h"

#define TWO_PI    6.283185307f
void pll_init(PLL* pll){
	pll->kp = 0.155478f;//0.0267412f;
	pll->ki = 0.01209f;//0.0302948f;
	pll->theta =0;
	pll->err_last =0;
	pll->addstop=false;
}

float pll_loop(float cos_theta,float sin_theta,PLL *pll){
	float err = -cos_theta*sinf(pll->theta) + sin_theta*cosf(pll->theta);
	float omiga_est = pll->kp*(err - pll->err_last) + err*pll->ki;
	pll->theta += omiga_est;
	pll->theta=fmod(pll->theta,TWO_PI);
	if(pll->theta<0) pll->theta+=TWO_PI;
	return omiga_est;
}
